#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose2D.h>
#include <iostream>
#include <vector>
#include <fstream>
#include <cmath>
#include <iomanip>

class OdometryCalibrationNode
{
private:
    ros::NodeHandle nh_;
    ros::Subscriber odom_sub_;
    
    nav_msgs::Odometry current_odom_;
    bool has_odom_data_;
    
    // 校准数据存储
    std::vector<double> actual_distances_;      // 实际移动距离
    std::vector<double> odom_distances_;       // 里程计移动距离
    std::vector<geometry_msgs::Pose2D> start_poses_;  // 起始位置
    std::vector<geometry_msgs::Pose2D> end_poses_;    // 结束位置
    
    // 模型参数
    double scale_factor_;    // 比例因子
    double offset_;          // 偏移量
    bool model_fitted_;      // 模型是否已拟合
    
    // 测试数据
    std::vector<double> test_actual_distances_;
    std::vector<double> test_predicted_distances_;
    std::vector<double> test_errors_;

public:
    OdometryCalibrationNode() : has_odom_data_(false), scale_factor_(1.0), 
                               offset_(0.0), model_fitted_(false)
    {
        // 订阅里程计话题
        odom_sub_ = nh_.subscribe("/odom", 10, &OdometryCalibrationNode::odomCallback, this);
        
        ROS_INFO("里程计校准节点已启动");
        ROS_INFO("请使用遥控器手动控制小车移动");
        
        // 显示操作菜单
        showMenu();
    }

    void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
    {
        // 检查里程计数据质量
        if (std::isnan(msg->pose.pose.position.x) || std::isnan(msg->pose.pose.position.y))
        {
            ROS_WARN("里程计数据包含NaN值，跳过此数据");
            return;
        }
        
        if (std::isinf(msg->pose.pose.position.x) || std::isinf(msg->pose.pose.position.y))
        {
            ROS_WARN("里程计数据包含无穷值，跳过此数据");
            return;
        }
        
        current_odom_ = *msg;
        has_odom_data_ = true;
        
        // 定期显示当前里程计状态（每10秒一次）
        static ros::Time last_print_time = ros::Time::now();
        if ((ros::Time::now() - last_print_time).toSec() > 10.0)
        {
            ROS_INFO("当前里程计位置: X=%.3f, Y=%.3f", 
                    current_odom_.pose.pose.position.x, 
                    current_odom_.pose.pose.position.y);
            last_print_time = ros::Time::now();
        }
    }

    void showMenu()
    {
        std::cout << "\n==================== 里程计校准系统 ====================" << std::endl;
        std::cout << "1. 记录起始位置 (小车静止时)" << std::endl;
        std::cout << "2. 记录结束位置 (移动后)" << std::endl;
        std::cout << "3. 添加校准数据点" << std::endl;
        std::cout << "4. 拟合校准模型" << std::endl;
        std::cout << "5. 显示当前模型参数" << std::endl;
        std::cout << "6. 测试距离计算" << std::endl;
        std::cout << "7. 显示校准数据" << std::endl;
        std::cout << "8. 清除所有数据" << std::endl;
        std::cout << "9. 保存校准数据" << std::endl;
        std::cout << "10. 加载校准数据" << std::endl;
        std::cout << "11. 里程计数据诊断" << std::endl;
        std::cout << "12. 退出程序" << std::endl;
        std::cout << "========================================================" << std::endl;
    }

    void run()
    {
        ros::Rate rate(10);
        
        while (ros::ok())
        {
            int choice;
            std::cout << "\n请选择操作 (1-12): ";
            std::cin >> choice;
            
            switch (choice)
            {
                case 1:
                    recordStartPosition();
                    break;
                case 2:
                    recordEndPosition();
                    break;
                case 3:
                    addCalibrationPoint();
                    break;
                case 4:
                    fitCalibrationModel();
                    break;
                case 5:
                    showModelParameters();
                    break;
                case 6:
                    testDistanceCalculation();
                    break;
                case 7:
                    showCalibrationData();
                    break;
                case 8:
                    clearAllData();
                    break;
                case 9:
                    saveCalibrationData();
                    break;
                case 10:
                    loadCalibrationData();
                    break;
                case 11:
                    diagnoseOdometryData();
                    break;
                case 12:
                    ROS_INFO("退出程序");
                    return;
                default:
                    std::cout << "无效选择，请重新输入" << std::endl;
                    break;
            }
            
            ros::spinOnce();
            rate.sleep();
        }
    }

    void recordStartPosition()
    {
        if (!has_odom_data_)
        {
            std::cout << "错误: 未收到里程计数据" << std::endl;
            return;
        }
        
        std::cout << "\n==================== 记录起始位置 ====================" << std::endl;
        std::cout << "请确保小车静止不动，按回车键记录起始位置...";
        std::cin.ignore();
        std::cin.get();
        
        // 等待数据稳定
        ros::Duration(0.5).sleep();
        
        geometry_msgs::Pose2D start_pose;
        start_pose.x = current_odom_.pose.pose.position.x;
        start_pose.y = current_odom_.pose.pose.position.y;
        start_pose.theta = getYawFromQuaternion(current_odom_.pose.pose.orientation);
        
        std::cout << "起始位置已记录:" << std::endl;
        std::cout << "  X: " << std::fixed << std::setprecision(3) << start_pose.x << " 米" << std::endl;
        std::cout << "  Y: " << std::fixed << std::setprecision(3) << start_pose.y << " 米" << std::endl;
        std::cout << "  角度: " << std::fixed << std::setprecision(3) << start_pose.theta << " 弧度" << std::endl;
        
        // 临时存储起始位置（用于添加校准点时使用）
        static geometry_msgs::Pose2D temp_start_pose;
        temp_start_pose = start_pose;
    }

    void recordEndPosition()
    {
        if (!has_odom_data_)
        {
            std::cout << "错误: 未收到里程计数据" << std::endl;
            return;
        }
        
        std::cout << "\n==================== 记录结束位置 ====================" << std::endl;
        std::cout << "小车移动完成后，按回车键记录结束位置...";
        std::cin.ignore();
        std::cin.get();
        
        // 等待数据稳定
        ros::Duration(0.5).sleep();
        
        geometry_msgs::Pose2D end_pose;
        end_pose.x = current_odom_.pose.pose.position.x;
        end_pose.y = current_odom_.pose.pose.position.y;
        end_pose.theta = getYawFromQuaternion(current_odom_.pose.pose.orientation);
        
        std::cout << "结束位置已记录:" << std::endl;
        std::cout << "  X: " << std::fixed << std::setprecision(3) << end_pose.x << " 米" << std::endl;
        std::cout << "  Y: " << std::fixed << std::setprecision(3) << end_pose.y << " 米" << std::endl;
        std::cout << "  角度: " << std::fixed << std::setprecision(3) << end_pose.theta << " 弧度" << std::endl;
        
        // 临时存储结束位置（用于添加校准点时使用）
        static geometry_msgs::Pose2D temp_end_pose;
        temp_end_pose = end_pose;
    }

    void addCalibrationPoint()
    {
        std::cout << "\n==================== 添加校准数据点 ====================" << std::endl;
        
        // 获取起始和结束位置
        geometry_msgs::Pose2D start_pose, end_pose;
        
        std::cout << "请输入起始位置 X: ";
        std::cin >> start_pose.x;
        std::cout << "请输入起始位置 Y: ";
        std::cin >> start_pose.y;
        
        std::cout << "请输入结束位置 X: ";
        std::cin >> end_pose.x;
        std::cout << "请输入结束位置 Y: ";
        std::cin >> end_pose.y;
        
        // 计算里程计移动距离
        double dx = end_pose.x - start_pose.x;
        double dy = end_pose.y - start_pose.y;
        double odom_distance = sqrt(dx*dx + dy*dy);
        
        // 获取实际移动距离
        double actual_distance;
        std::cout << "请输入实际移动距离 (米): ";
        std::cin >> actual_distance;
        
        // 存储数据
        start_poses_.push_back(start_pose);
        end_poses_.push_back(end_pose);
        actual_distances_.push_back(actual_distance);
        odom_distances_.push_back(odom_distance);
        
        std::cout << "\n校准数据点已添加:" << std::endl;
        std::cout << "  实际距离: " << std::fixed << std::setprecision(3) << actual_distance << " 米" << std::endl;
        std::cout << "  里程计距离: " << std::fixed << std::setprecision(3) << odom_distance << " 米" << std::endl;
        std::cout << "  比例: " << std::fixed << std::setprecision(3) << (actual_distance / odom_distance) << std::endl;
        std::cout << "  当前数据点总数: " << actual_distances_.size() << std::endl;
    }

    void fitCalibrationModel()
    {
        std::cout << "\n==================== 拟合校准模型 ====================" << std::endl;
        
        if (actual_distances_.size() < 2)
        {
            std::cout << "错误: 数据点不足，至少需要2个数据点才能拟合模型" << std::endl;
            std::cout << "当前数据点数: " << actual_distances_.size() << std::endl;
            return;
        }
        
        // 显示所有校准数据
        std::cout << "校准数据点:" << std::endl;
        for (size_t i = 0; i < actual_distances_.size(); i++)
        {
            std::cout << "  点" << (i+1) << ": 实际=" << std::fixed << std::setprecision(3) 
                     << actual_distances_[i] << "米, 里程计=" << odom_distances_[i] 
                     << "米, 比例=" << (actual_distances_[i] / odom_distances_[i]) << std::endl;
        }
        
        // 使用最小二乘法拟合线性模型: actual = scale_factor * odom + offset
        double sum_x = 0, sum_y = 0, sum_xy = 0, sum_x2 = 0;
        int n = actual_distances_.size();
        
        for (int i = 0; i < n; i++)
        {
            double x = odom_distances_[i];
            double y = actual_distances_[i];
            
            sum_x += x;
            sum_y += y;
            sum_xy += x * y;
            sum_x2 += x * x;
        }
        
        // 计算线性回归参数
        double denominator = n * sum_x2 - sum_x * sum_x;
        if (fabs(denominator) < 1e-10)
        {
            std::cout << "错误: 数据点共线或重复，无法拟合模型" << std::endl;
            return;
        }
        
        scale_factor_ = (n * sum_xy - sum_x * sum_y) / denominator;
        offset_ = (sum_y - scale_factor_ * sum_x) / n;
        
        model_fitted_ = true;
        
        std::cout << "\n模型拟合完成:" << std::endl;
        std::cout << "模型方程: 实际距离 = " << std::fixed << std::setprecision(6) 
                 << scale_factor_ << " * 里程计距离 + " << offset_ << std::endl;
        
        // 计算拟合精度
        double total_error = 0;
        double max_error = 0;
        int max_error_idx = 0;
        
        std::cout << "\n拟合精度分析:" << std::endl;
        for (int i = 0; i < n; i++)
        {
            double predicted = scale_factor_ * odom_distances_[i] + offset_;
            double error = fabs(predicted - actual_distances_[i]);
            double relative_error = (error / actual_distances_[i]) * 100;
            total_error += error;
            
            if (error > max_error)
            {
                max_error = error;
                max_error_idx = i;
            }
            
            std::cout << "  测量" << (i+1) << ": 实际=" << std::fixed << std::setprecision(3) 
                     << actual_distances_[i] << "米, 预测=" << predicted 
                     << "米, 绝对误差=" << error << "米, 相对误差=" 
                     << std::fixed << std::setprecision(1) << relative_error << "%" << std::endl;
        }
        
        double avg_error = total_error / n;
        double r_squared = calculateRSquared();
        
        std::cout << "\n统计信息:" << std::endl;
        std::cout << "  平均绝对误差: " << std::fixed << std::setprecision(3) << avg_error << " 米" << std::endl;
        std::cout << "  最大绝对误差: " << std::fixed << std::setprecision(3) << max_error 
                 << " 米 (测量" << (max_error_idx+1) << ")" << std::endl;
        std::cout << "  决定系数 R²: " << std::fixed << std::setprecision(3) << r_squared << std::endl;
        
        if (r_squared < 0.8)
        {
            std::cout << "警告: R² < 0.8，模型拟合质量较差" << std::endl;
            std::cout << "建议: 增加更多不同距离的校准数据" << std::endl;
        }
        else if (r_squared > 0.95)
        {
            std::cout << "良好: R² > 0.95，模型拟合质量很好" << std::endl;
        }
    }

    double calculateRSquared()
    {
        if (actual_distances_.size() < 2) return 0.0;
        
        double mean_y = 0;
        for (double y : actual_distances_)
        {
            mean_y += y;
        }
        mean_y /= actual_distances_.size();
        
        double ss_tot = 0, ss_res = 0;
        for (size_t i = 0; i < actual_distances_.size(); i++)
        {
            double y_actual = actual_distances_[i];
            double y_pred = scale_factor_ * odom_distances_[i] + offset_;
            
            ss_tot += (y_actual - mean_y) * (y_actual - mean_y);
            ss_res += (y_actual - y_pred) * (y_actual - y_pred);
        }
        
        return 1.0 - (ss_res / ss_tot);
    }

    void showModelParameters()
    {
        std::cout << "\n==================== 模型参数 ====================" << std::endl;
        if (model_fitted_)
        {
            std::cout << "模型方程: 实际距离 = " << std::fixed << std::setprecision(6) 
                     << scale_factor_ << " * 里程计距离 + " << offset_ << std::endl;
            std::cout << "校准数据点数: " << actual_distances_.size() << std::endl;
            
            if (test_actual_distances_.size() > 0)
            {
                std::cout << "测试数据点数: " << test_actual_distances_.size() << std::endl;
                
                // 计算测试误差统计
                double total_error = 0;
                for (size_t i = 0; i < test_actual_distances_.size(); i++)
                {
                    total_error += test_errors_[i];
                }
                double avg_error = total_error / test_actual_distances_.size();
                std::cout << "平均测试误差: " << std::fixed << std::setprecision(3) << avg_error << " 米" << std::endl;
            }
        }
        else
        {
            std::cout << "模型尚未建立，请先进行校准" << std::endl;
        }
    }

    void testDistanceCalculation()
    {
        if (!model_fitted_)
        {
            std::cout << "错误: 请先拟合校准模型" << std::endl;
            return;
        }
        
        std::cout << "\n==================== 测试距离计算 ====================" << std::endl;
        
        // 获取起始和结束位置
        geometry_msgs::Pose2D start_pose, end_pose;
        
        std::cout << "请输入起始位置 X: ";
        std::cin >> start_pose.x;
        std::cout << "请输入起始位置 Y: ";
        std::cin >> start_pose.y;
        
        std::cout << "请输入结束位置 X: ";
        std::cin >> end_pose.x;
        std::cout << "请输入结束位置 Y: ";
        std::cin >> end_pose.y;
        
        // 计算里程计移动距离
        double dx = end_pose.x - start_pose.x;
        double dy = end_pose.y - start_pose.y;
        double odom_distance = sqrt(dx*dx + dy*dy);
        
        // 使用模型预测实际距离
        double predicted_distance = scale_factor_ * odom_distance + offset_;
        
        // 获取实际距离
        double actual_distance;
        std::cout << "请输入实际移动距离 (米): ";
        std::cin >> actual_distance;
        
        // 验证输入
        if (actual_distance <= 0)
        {
            std::cout << "错误: 实际距离必须大于0" << std::endl;
            std::cout << "请使用卷尺等工具精确测量实际移动距离" << std::endl;
            return;
        }
        
        if (actual_distance > 100)
        {
            std::cout << "警告: 实际距离过大 (" << actual_distance << " 米)" << std::endl;
            std::cout << "请确认测量是否正确" << std::endl;
        }
        
        // 计算误差
        double error = fabs(predicted_distance - actual_distance);
        double error_percentage = (error / actual_distance) * 100;
        
        std::cout << "\n测试结果:" << std::endl;
        std::cout << "  实际距离: " << std::fixed << std::setprecision(3) << actual_distance << " 米" << std::endl;
        std::cout << "  里程计距离: " << std::fixed << std::setprecision(3) << odom_distance << " 米" << std::endl;
        std::cout << "  预测距离: " << std::fixed << std::setprecision(3) << predicted_distance << " 米" << std::endl;
        std::cout << "  绝对误差: " << std::fixed << std::setprecision(3) << error << " 米" << std::endl;
        std::cout << "  相对误差: " << std::fixed << std::setprecision(1) << error_percentage << "%" << std::endl;
        
        // 保存测试数据
        test_actual_distances_.push_back(actual_distance);
        test_predicted_distances_.push_back(predicted_distance);
        test_errors_.push_back(error);
    }

    void showCalibrationData()
    {
        std::cout << "\n==================== 校准数据 ====================" << std::endl;
        std::cout << "校准数据点总数: " << actual_distances_.size() << std::endl;
        
        if (actual_distances_.size() > 0)
        {
            std::cout << "数据点详情:" << std::endl;
            for (size_t i = 0; i < actual_distances_.size(); i++)
            {
                std::cout << "  点" << (i+1) << ":" << std::endl;
                std::cout << "    起始位置: (" << std::fixed << std::setprecision(3) 
                         << start_poses_[i].x << ", " << start_poses_[i].y << ")" << std::endl;
                std::cout << "    结束位置: (" << std::fixed << std::setprecision(3) 
                         << end_poses_[i].x << ", " << end_poses_[i].y << ")" << std::endl;
                std::cout << "    实际距离: " << std::fixed << std::setprecision(3) 
                         << actual_distances_[i] << " 米" << std::endl;
                std::cout << "    里程计距离: " << std::fixed << std::setprecision(3) 
                         << odom_distances_[i] << " 米" << std::endl;
                std::cout << "    比例: " << std::fixed << std::setprecision(3) 
                         << (actual_distances_[i] / odom_distances_[i]) << std::endl;
            }
        }
        
        if (test_actual_distances_.size() > 0)
        {
            std::cout << "\n测试数据点总数: " << test_actual_distances_.size() << std::endl;
            std::cout << "测试数据详情:" << std::endl;
            for (size_t i = 0; i < test_actual_distances_.size(); i++)
            {
                std::cout << "  测试" << (i+1) << ": 实际=" << std::fixed << std::setprecision(3) 
                         << test_actual_distances_[i] << "米, 预测=" << test_predicted_distances_[i] 
                         << "米, 误差=" << test_errors_[i] << "米" << std::endl;
            }
        }
    }

    void clearAllData()
    {
        std::cout << "\n==================== 清除所有数据 ====================" << std::endl;
        std::cout << "确定要清除所有数据吗？这将删除:" << std::endl;
        std::cout << "- 所有校准数据点" << std::endl;
        std::cout << "- 拟合的模型参数" << std::endl;
        std::cout << "- 所有测试数据" << std::endl;
        std::cout << "输入 'yes' 确认清除，其他任意键取消: ";
        
        std::string confirm;
        std::cin >> confirm;
        
        if (confirm == "yes" || confirm == "YES" || confirm == "Yes")
        {
            actual_distances_.clear();
            odom_distances_.clear();
            start_poses_.clear();
            end_poses_.clear();
            test_actual_distances_.clear();
            test_predicted_distances_.clear();
            test_errors_.clear();
            
            scale_factor_ = 1.0;
            offset_ = 0.0;
            model_fitted_ = false;
            
            std::cout << "所有数据已清除，可以重新开始校准" << std::endl;
        }
        else
        {
            std::cout << "操作已取消" << std::endl;
        }
    }

    void saveCalibrationData()
    {
        if (!model_fitted_)
        {
            std::cout << "没有校准数据可保存" << std::endl;
            return;
        }
        
        std::ofstream file("odometry_calibration_data.txt");
        if (file.is_open())
        {
            file << "Odometry Calibration Data" << std::endl;
            file << "Scale Factor: " << std::fixed << std::setprecision(6) << scale_factor_ << std::endl;
            file << "Offset: " << std::fixed << std::setprecision(6) << offset_ << std::endl;
            file << "Calibration Points:" << std::endl;
            
            for (size_t i = 0; i < actual_distances_.size(); i++)
            {
                file << actual_distances_[i] << " " << odom_distances_[i] << std::endl;
            }
            
            file.close();
            std::cout << "校准数据已保存到 odometry_calibration_data.txt" << std::endl;
        }
        else
        {
            std::cout << "无法创建文件" << std::endl;
        }
    }

    void loadCalibrationData()
    {
        std::ifstream file("odometry_calibration_data.txt");
        if (file.is_open())
        {
            std::string line;
            actual_distances_.clear();
            odom_distances_.clear();
            
            while (std::getline(file, line))
            {
                if (line.find("Scale Factor:") != std::string::npos)
                {
                    sscanf(line.c_str(), "Scale Factor: %lf", &scale_factor_);
                }
                else if (line.find("Offset:") != std::string::npos)
                {
                    sscanf(line.c_str(), "Offset: %lf", &offset_);
                }
                else if (line.find("Calibration Points:") != std::string::npos)
                {
                    // 跳过标题行
                }
                else if (!line.empty())
                {
                    double actual, odom;
                    if (sscanf(line.c_str(), "%lf %lf", &actual, &odom) == 2)
                    {
                        actual_distances_.push_back(actual);
                        odom_distances_.push_back(odom);
                    }
                }
            }
            
            file.close();
            model_fitted_ = true;
            std::cout << "校准数据已加载" << std::endl;
            showModelParameters();
        }
        else
        {
            std::cout << "无法打开文件 odometry_calibration_data.txt" << std::endl;
        }
    }

    void diagnoseOdometryData()
    {
        std::cout << "\n==================== 里程计数据诊断 ====================" << std::endl;
        
        if (!has_odom_data_)
        {
            std::cout << "错误: 未收到里程计数据" << std::endl;
            std::cout << "请检查:" << std::endl;
            std::cout << "1. 底盘是否正常启动" << std::endl;
            std::cout << "2. 里程计话题是否正常发布" << std::endl;
            std::cout << "3. 网络连接是否正常" << std::endl;
            return;
        }
        
        std::cout << "当前里程计状态:" << std::endl;
        std::cout << "  位置 X: " << std::fixed << std::setprecision(6) 
                 << current_odom_.pose.pose.position.x << " 米" << std::endl;
        std::cout << "  位置 Y: " << std::fixed << std::setprecision(6) 
                 << current_odom_.pose.pose.position.y << " 米" << std::endl;
        std::cout << "  位置 Z: " << std::fixed << std::setprecision(6) 
                 << current_odom_.pose.pose.position.z << " 米" << std::endl;
        
        std::cout << "  线速度 X: " << std::fixed << std::setprecision(6) 
                 << current_odom_.twist.twist.linear.x << " m/s" << std::endl;
        std::cout << "  线速度 Y: " << std::fixed << std::setprecision(6) 
                 << current_odom_.twist.twist.linear.y << " m/s" << std::endl;
        std::cout << "  角速度 Z: " << std::fixed << std::setprecision(6) 
                 << current_odom_.twist.twist.angular.z << " rad/s" << std::endl;
        
        // 数据质量检查
        std::cout << "\n数据质量检查:" << std::endl;
        
        bool has_issues = false;
        
        // 检查位置数据
        if (std::isnan(current_odom_.pose.pose.position.x) || std::isnan(current_odom_.pose.pose.position.y))
        {
            std::cout << "  ❌ 位置数据包含NaN值" << std::endl;
            has_issues = true;
        }
        else
        {
            std::cout << "  ✅ 位置数据正常" << std::endl;
        }
        
        if (std::isinf(current_odom_.pose.pose.position.x) || std::isinf(current_odom_.pose.pose.position.y))
        {
            std::cout << "  ❌ 位置数据包含无穷值" << std::endl;
            has_issues = true;
        }
        
        // 检查速度数据
        if (std::isnan(current_odom_.twist.twist.linear.x) || std::isnan(current_odom_.twist.twist.linear.y))
        {
            std::cout << "  ❌ 速度数据包含NaN值" << std::endl;
            has_issues = true;
        }
        else
        {
            std::cout << "  ✅ 速度数据正常" << std::endl;
        }
        
        // 检查位置是否异常大
        double position_magnitude = sqrt(current_odom_.pose.pose.position.x * current_odom_.pose.pose.position.x + 
                                        current_odom_.pose.pose.position.y * current_odom_.pose.pose.position.y);
        if (position_magnitude > 1000.0)
        {
            std::cout << "  ❌ 位置数据异常大 (" << position_magnitude << " 米)" << std::endl;
            has_issues = true;
        }
        else
        {
            std::cout << "  ✅ 位置数据大小正常" << std::endl;
        }
        
        // 检查速度是否异常大
        double velocity_magnitude = sqrt(current_odom_.twist.twist.linear.x * current_odom_.twist.twist.linear.x + 
                                        current_odom_.twist.twist.linear.y * current_odom_.twist.twist.linear.y);
        if (velocity_magnitude > 10.0)
        {
            std::cout << "  ❌ 速度数据异常大 (" << velocity_magnitude << " m/s)" << std::endl;
            has_issues = true;
        }
        else
        {
            std::cout << "  ✅ 速度数据大小正常" << std::endl;
        }
        
        std::cout << "\n建议操作:" << std::endl;
        if (has_issues)
        {
            std::cout << "1. 重启底盘节点" << std::endl;
            std::cout << "2. 检查硬件连接" << std::endl;
            std::cout << "3. 检查传感器状态" << std::endl;
            std::cout << "4. 重新进行校准" << std::endl;
        }
        else
        {
            std::cout << "1. 里程计数据正常，可以继续校准" << std::endl;
            std::cout << "2. 建议清除旧数据重新开始" << std::endl;
        }
        
        std::cout << "\n按回车键继续...";
        std::cin.ignore();
        std::cin.get();
    }

    double getYawFromQuaternion(const geometry_msgs::Quaternion& quat)
    {
        return atan2(2.0 * (quat.w * quat.z + quat.x * quat.y), 
                     1.0 - 2.0 * (quat.y * quat.y + quat.z * quat.z));
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "odometry_calibration_node");
    
    // 设置控制台编码为UTF-8以正确显示中文
    setlocale(LC_ALL, "zh_CN.UTF-8");
    
    OdometryCalibrationNode node;
    node.run();
    
    return 0;
}
